#!/usr/bin/python3.8

import time
import minimalmodbus
from serial import Serial


class Claw:
    def init(self, force=80, dist=80):
        port = Serial("COM1", baudrate=9600, parity="E", timeout=0.1)
        self.client = minimalmodbus.Instrument(port, 1)

        self.client.write_register(0x0501, 1, functioncode=6)
        self.client.write_register(0x0100, 1, functioncode=6)
        while 1 != self.client.read_register(0x0200):
            time.sleep(1)
        self.client.write_register(0x0104, 80, functioncode=6)
        self.client.write_register(0x030A, 2, functioncode=6)
        self.client.write_register(0x0101, force, functioncode=6)
        self.client.write_register(0x0102, dist * 100, functioncode=6)

    def setPos(self, val: int):
        self.client.write_register(0x0103, val * 100, functioncode=6)

    def open(self):
        self.setPos(80)

    def close(self):
        self.setPos(0)

    def getState(self):
        state, pos, cur = self.client.read_registers(0x0201, 3)
        return state, pos / 100, cur


port = Serial("COM1", baudrate=9600, parity="E", timeout=0.1)
client = minimalmodbus.Instrument(port, 1)


def claw(close, force=150, addr=0x01):
    # type: (bool, int, int)->bool
    client.write_register(addr, force if close else 0, functioncode=6)
    time.sleep(5)
    client.write_register(addr, 1, functioncode=6)  # 放空
    return True


if __name__ == "__main__":
    # port = Serial("COM1", baudrate=9600, parity="E", timeout=0.1)
    # client = minimalmodbus.Instrument(port, 1)
    cnt = 0
    claw(True)
    # client.write_register(0x01, 1, functioncode=6)
    # while True:
    #     pos = client.read_registers(0x02, 1)
    #     print(pos[0])
    #     if pos[0] < 120:
    #         time.sleep(2)
    #         print("====", pos[0])
    #         client.write_register(0x00, 2, functioncode=6)
    #     if pos[0] > 165:
    #         time.sleep(2)
    #         print("====", pos[0])
    #         client.write_register(0x00, 1, functioncode=6)
    #     time.sleep(0.1)
